using System;
using System.Threading;
using System.Collections.Generic;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Audio;
using Microsoft.Xna.Framework.Content;
using Microsoft.Xna.Framework.GamerServices;
using Microsoft.Xna.Framework.Graphics;
using Microsoft.Xna.Framework.Input;
using Microsoft.Xna.Framework.Net;
using Microsoft.Xna.Framework.Storage;

namespace Autobot
{

    /// <summary>
    /// This is the main type for your game
    /// </summary>
    public class AutobotRunner : Microsoft.Xna.Framework.Game
    {
        
        // For XNA drawing
        private GraphicsDeviceManager graphics;
        private SpriteBatch spriteBatch;

        // Define each state of the program
        enum SimulatorState
        {
            STARTUP = 0,          // Initial state of the program
            PLACE_GOAL,			  // User specifies where the goal is before the runner starts
            RUN,                  // The robot is running an algorithm to reach its destination
            STOP,                 // Stop the car for whatever reason, either for goal success or panic
            SHUT_DOWN,            // The program is finished so shut down
            NUM_STATES
        }

        // Remember the current state of the program
        private SimulatorState state;

        //This is the object that will visually describe the robot and its movement
        //along with the goal and the destination
        private Observer observer;

        // Remember the user's input from the last update
        private KeyboardState lastKeyboardInput;
        private MouseState lastMouseInput;

       
        // Hold an algorithm base class
        private Algorithms algorithms; // Controller for algorithms
        private AlgorithmBase algorithm; // Specific algorithm that will be used

        // Hold a custom controller class
        private RobotController controller;

        private CommunicatorServer communicator;
        private LogController communicator_log;

        #region Autobot Runner Constructors
        // Default input parameters
        private const int DefaultWindowWidth = 800,
                          DefaultWindowHeight = 600;
        private const string DefaultAlgorithm = "UserInput";
        // Default constructor: Assume screen dimensions of 1024 and 768
        public AutobotRunner()
        {
            this.InitRunner(DefaultWindowWidth, DefaultWindowHeight, DefaultAlgorithm);
        }

        // Arguments passed: What algorithm to use
        public AutobotRunner(string algorithm)
        {
            this.InitRunner(DefaultWindowWidth, DefaultWindowHeight, algorithm);
        }

        // Arguments passed: Screen dimensions
        public AutobotRunner(int width, int height)
        {
            this.InitRunner(width, height, DefaultAlgorithm);
        }

        // Arguments passed: Screen dimensions and algorithm to use
        public AutobotRunner(int width, int height, string selectedAlgorithm)
        {
            this.InitRunner(width, height, selectedAlgorithm);
        }
        #endregion

        // Initialize Autobot Runner
        public void InitRunner(int width, int height, string selectedAlgorithm)
        {
            // Set the program state to StartUp
            state = SimulatorState.STARTUP;
            
            // Load up the graphics manager, set the resolution
            graphics = new GraphicsDeviceManager(this);
            graphics.PreferredBackBufferHeight = height;
            graphics.PreferredBackBufferWidth = width;
            Window.Title = "Autobot Runner";

            // Set the content directory to /Content
            Content.RootDirectory = "Content";

            // Set up the observer window which contains Robot, goal and map
            observer = new Observer(new Vector2(width, height));

            // Create a new logfile for the communicator
            communicator_log = new LogController();
            bool success = communicator_log.OpenLog("ServerLog.txt");
            if (!success)
            {
                Console.WriteLine("ERROR: could not open communicator_log.txt for writing");
                this.Exit();
            }

            // Set up the communicator
            communicator = new CommunicatorServer();
            
            // Initialize it, quit if it doesn't succeed
            if (!communicator.Initialize(communicator_log, null))
                this.Exit();

            // Set up the algorithm
            // Load up the algorithms controller
            algorithms = new Algorithms(observer.GetMap(), observer.GetRobot(), observer.GetDestination());
            // Get the algorithm to use
            algorithm = algorithms.GetAlgorithm(selectedAlgorithm);
            // If algorithm is null, throw an exception
            if (algorithm == null)
            {
                Popup.Show("Could not load " + selectedAlgorithm + " algorithm");
                this.Exit();
            }

            // Set up the controller to use the communicator
            controller = new RobotController(communicator, "CommandLog.txt");

        }
        /// <summary>
        /// Allows the game to perform any initialization it needs to before starting to run.
        /// This is where it can query for any required services and load any non-graphic
        /// related content.  Calling base.Initialize will enumerate through any components
        /// and initialize them as well.
        /// </summary>
        protected override void Initialize()
        {
            // Set the last states to the current keyboard/mouse states
            lastKeyboardInput = Keyboard.GetState();
            lastMouseInput = Mouse.GetState();

            // Make the mouse visible
            this.IsMouseVisible = true;
     
            base.Initialize();
        }

        /// <summary>
        /// LoadContent will be called once per game and is the place to load
        /// all of your content.
        /// </summary>
        protected override void LoadContent()
        {
            // Create a new SpriteBatch, which can be used to draw textures.
            spriteBatch = new SpriteBatch(GraphicsDevice);

            // Load Observer content
            observer.LoadContent(Content, GraphicsDevice);
        }

        /// <summary>
        /// UnloadContent will be called once per game and is the place to unload
        /// all content.
        /// </summary>
        protected override void UnloadContent()
        {
            // Dispose of owned objects, including sprite batches
            observer.Dispose();
            if(!spriteBatch.IsDisposed)
                spriteBatch.Dispose();

            // Close the communicator
            communicator.Close();
            communicator_log.CloseLog();
        }
        
        /// <summary>
        /// Allows the game to run logic such as updating the world,
        /// checking for collisions, gathering input, and playing audio.
        /// </summary>
        /// <param name="gameTime">Provides a snapshot of timing values.</param>
        protected override void Update(GameTime gameTime)
        {
            // Get user input
            KeyboardState keyboardInput = Keyboard.GetState();
            MouseState mouseInput = Mouse.GetState();

            //Check the escape key for when the user wishes to exit
            if (keyboardInput.IsKeyDown(Keys.Escape))
                this.Exit();

            // User input is based on what state the program is in. Execute the approrpriate update method based
            // on what state the program is in.
            switch (state)
            {
                case SimulatorState.STARTUP:
                    #region Autobot Runner Start Up
                    // Get the starting position of the car from the wii-mote.
                    // Init The motors and the sensors.
                    // Set the position of the destination
                    // Kick off the simulator
                    state = SimulatorState.PLACE_GOAL;
                    #endregion
                    break;

                case SimulatorState.PLACE_GOAL:
                    #region User specifies the location of the goal
                    // Only process input if the window is active
                    if (IsActive)
                    {
                        // If the user is holding down the left mouse button 
                        if (mouseInput.LeftButton == ButtonState.Pressed && mouseInput.RightButton == ButtonState.Released)
                        {
                            // Calculate the goal's real position
                            Vector2 mousePosition = new Vector2(mouseInput.X, mouseInput.Y);
                            // Place the goal where the mouse pointer is
                            observer.GetDestination().SetPosition(mousePosition, observer.GetMap().GetRealPosition(mousePosition));
                        }
                        // If the user pushes the right mouse button, start the program!
                        if (mouseInput.RightButton == ButtonState.Pressed && mouseInput.LeftButton == ButtonState.Released)
                        {
                            Popup.Show("Goal placed!");
                            state = SimulatorState.RUN;
                        }
                    }
                    #endregion
                    break;
               
                case SimulatorState.RUN:
                    // Here we will parse one message at a time. Depending on the type of message,
                    // different robot update information fields will be filled completely, others will
                    // be set to null. The null cases are dealt with in each of the subclass update functions
                    // so setting them to null shouldn't be dangerous. This gives us some flexibility so we don't 
                    // have to wait until all the information is present before the algorithm can choose what to 
                    // do next. All types of information are transmitted about 4 times a second so we can sacrifice
                    // a little precision for performance.
                    

                    // We should do a check to see how many messages are in the queue.
                    // This will give us an idea of how behind the current message is. Given the current
                    // situation where the processor on the car is really slow compared to the host machine,
                    // these messages shouldn't overflow. But we should check anyways.
                    Message nextMessage;
                    nextMessage = communicator.GetNextMessage();

                    // Check if we have reached the goal
                    if (observer.CheckGoalReached())
                    {
                        state = SimulatorState.STOP;
                    }
                    if (nextMessage != null)
                    {
                        communicator_log.WriteLog("The next message is " + nextMessage.SaveToString());

                        // Here we only care about the Client to Server messages. This is found in message.cs
                        // ROBOT_DIRECTION,  This is to update the robot's direction    Params: {float X} {float Y}             Usage: Client to Server
                        // ROBOT_POSITION,  This is to set the robot position           Params: {float X} {float Y}             Usage: Client to Server
                        // SENSOR_UPDATE,   Update in the sensor readings               Params: {int WhichSensor} {float Value} Usage: Client to Server
                        // MOTOR_UPDATE,    Update in the motor value                   Params: {int WhichMotor} {int value}    Usage: Client to Server

                        int[] MotorSettings = null;
                        float[] SensorReadings = null;
                        Vector2 NewPosition = new Vector2(-1.0f,-1.0f);
                        Vector2 NewDirection = new Vector2(-1.0f,-1.0f);
                        float NewVelocity = 0.0f;

                        switch ((int)nextMessage.GetMessageType())
                        {    
                            case (int)MessageType.ROBOT_UPDATE:
                                // Message params should be X and y components as floats
                                try
                                {
                                    // Get the new position
                                    NewPosition = new Vector2(Convert.ToSingle(nextMessage.GetField("PosX")), Convert.ToSingle(nextMessage.GetField("PosY")));
                                    Console.Write("X: " + NewPosition.X + " Y: " + NewPosition.Y);

                                    // The position is between 0-10f, conver it into values that enter the operaiton area of the robot
                                    NewPosition.X *= (observer.GetMap().GetRealSize().X / 10.0f);
                                    NewPosition.Y *= (observer.GetMap().GetRealSize().Y / 10.0f);
                                  
                                    // Get the new direction
                                    NewDirection = new Vector2(Convert.ToSingle(nextMessage.GetField("DirX")), Convert.ToSingle(nextMessage.GetField("DirY")));
                                    
                                    // Get the new motor settings
                                    MotorSettings = new int[(int)MotorID.NUM_MOTORS];
                                    MotorSettings[(int)MotorID.DRIVE] = Convert.ToInt32(nextMessage.GetField("Drive"));
                                    MotorSettings[(int)MotorID.STEERING] = Convert.ToInt32(nextMessage.GetField("Steer"));

                                    // Get the new sensor readings
                                    SensorReadings = new float[(int)SensorID.NUM_SENSORS];
                                    SensorReadings[(int)SensorID.LEFT] = Convert.ToSingle(nextMessage.GetField("Left"));
                                    SensorReadings[(int)SensorID.RIGHT] = Convert.ToSingle(nextMessage.GetField("Right"));
                                    SensorReadings[(int)SensorID.MIDDLE] = Convert.ToSingle(nextMessage.GetField("Middle"));

                                    if (SensorReadings[(int)SensorID.LEFT] > 0.96)
                                        SensorReadings[(int)SensorID.LEFT] = 1.0f;
                                    if (SensorReadings[(int)SensorID.RIGHT] > 0.96)
                                        SensorReadings[(int)SensorID.RIGHT] = 1.0f;
                                    if (SensorReadings[(int)SensorID.MIDDLE] > 0.96)
                                        SensorReadings[(int)SensorID.MIDDLE] = 1.0f;

                                    NewVelocity = Convert.ToSingle(nextMessage.GetField("Velocity"));

                                    // Update the observer and so on... Observer will update the robot and the map
                                    observer.Update(gameTime, NewPosition, NewDirection, NewVelocity, SensorReadings, MotorSettings);

                                }
                                catch (FormatException)
                                {
                                    break;
                                }
                                break;
                            case (int)MessageType.ALGORITHM_UPDATE:
                                // Update the algorithm with the new real robot data.
                                AlgorithmResult NewResult = algorithm.GetNextResult();

                                // Pass the result to the controller
                                controller.Update(gameTime, NewResult);
                                break;
                            
                            default:
                                break;
                        }
                        
                    }
                    // See if the robot has reached the goal
                    if((observer.GetDestination().GetRealPosition() - observer.GetRobot().GetRealPosition()).Length() < 0.25)
                    {
                        // Give the success message
                        Popup.Show("Reached the goal! yeeeees");
                        // Save the map
                        observer.GetMap().SetReadOnly();
                        // Save the drawn map
                        observer.GetMap().SaveMap("SuccessMap.map");
                        state = SimulatorState.STOP;
                    }
                    // If we didn't get a new message, nothing changed so we don't need to do anything. Right? lol
                    break;
                
                case SimulatorState.STOP:
                    // Update the car with stoppage if not stopped already
                    if (observer.GetRobot().GetMotorSetting((int)MotorID.DRIVE) != (int)DriveMotorParams.STOP)
                    {
                        // Simulator will only listen if you give it a burst begin and end
                        Message bBegin = new Message();
                        bBegin.CreateBurstBeginMsg(1, 1);

                        Message stop = new Message();
                        stop.SetClientID(1);
                        stop.SetMessageType(MessageType.MOTOR_SET);
                        stop.CreateMotorSetMsg(1, (int)MotorID.DRIVE, (int)DriveMotorParams.STOP, 0);

                        Message bEnd = new Message();
                        bEnd.CreateBurstEndMsg(1);

                        // Send a stop message to the robot.
                        communicator.SendMessage(1, bBegin);
                        communicator.SendMessage(1, stop);
                        communicator.SendMessage(1, bEnd);
                    }
                    
                    break;
                case SimulatorState.SHUT_DOWN:
                    //Make sure car is stopped
                    break;
                
                default: throw new Exception("Invalid state");
            }

            // Remember the current input as the input from the previous update
            lastKeyboardInput = keyboardInput;
            lastMouseInput = mouseInput;

            base.Update(gameTime);
        }

        /// <summary>
        /// This is called when the game should draw itself.
        /// </summary>
        /// <param name="gameTime">Provides a snapshot of timing values.</param>
        protected override void Draw(GameTime gameTime)
        {
            // Always clear the graphics before drawing anything
            graphics.GraphicsDevice.Clear(Color.CornflowerBlue);
    
            //Draw the observer
            observer.Draw();

            // Begin Drawing whatever to screen. This could be deleted if not needed
            spriteBatch.Begin(SpriteBlendMode.AlphaBlend);
                        
            // End drawing to screen
            spriteBatch.End();    

            base.Draw(gameTime);
        }

    } 
}